/** ###################################################################
**     Filename  : Drv.C
**     Project   : CyberPark
**     Processor : MC9S12XS128MAL
**     Compiler  : CodeWarrior HCS12X C Compiler
**     Date/Time : 2011-7-31, 23:03
**     Contents  :
**         User source code
**
**     (c) Copyright UNIS, a.s. 1997-2008
**     UNIS, a.s.
**     Jundrovska 33
**     624 00 Brno
**     Czech Republic
**     http      : www.processorexpert.com
**     mail      : info@processorexpert.com
** ###################################################################*/

/* MODULE Drv */
#include "Drv.h"
#include "Pid.h"
#include "Hmi.h"


#include "Drv.h"
#include "Pid.h"
#include "Hmi.h"


U16 DrvServoMiddle = DRV_SERVO_MIDDLE_DEFAULT;
U16 DrvServoLeft = DRV_SERVO_LEFT_DEFAULT;
U16 DrvServoRight = DRV_SERVO_RIGHT_DEFAULT;
U16 DrvServoTarget = DRV_SERVO_MIDDLE_DEFAULT;



#define HISTORY_MAX (5)

U8  DrvFrozen=0;
S16 DrvSavedExpect;
U16 DrvSavedCurrent;



typedef struct{
	S16 expect;
	U16 current;
	U16 history[HISTORY_MAX];
	mc_sPIDparams param;
} DRV_MOTOR_CONTEXT;

DRV_MOTOR_CONTEXT DrvpMotorContext;


volatile int DrvpEnableControl = 0;

static void DrvpInitializePidController(mc_sPIDparams *param)
{
	param->ProportionalGain = 1536;
	param->ProportionalGainScale = 3;
	param->IntegralGain = 1536;
	param->IntegralGainScale = 5;
	param->DerivativeGain = 0;
	param->DerivativeGainScale = 11;
	param->PositivePIDLimit = 32767;
	param->NegativePIDLimit = -32767;
	param->IntegralPortionK_1 = 0;
	param->InputErrorK_1 = 0;
	return;
}
static void DrvpInitializePidControllerFront(mc_sPIDparams *param){
    param->ProportionalGain = 1536;
	param->ProportionalGainScale = 3;
	param->IntegralGain = 1536;
	param->IntegralGainScale = 5;
	param->DerivativeGain = 1536;
	param->DerivativeGainScale = 11;
	param->PositivePIDLimit = 32767;
	param->NegativePIDLimit = 0;
	param->IntegralPortionK_1 = 0;
	param->InputErrorK_1 = 0;    
	return;
}

void DrvSetPidChoice(U8 choice){
	DRV_MOTOR_CONTEXT *mc;
	mc=&DrvpMotorContext;   
    switch(choice){
        case DRV_FRONT: 
        {
            DrvpInitializePidControllerFront(&mc->param);    
            
        }
            break;
        case DRV_FRONT_BACK:
        {
            DrvpInitializePidController(&mc->param);   
            
        }
            break;
        
    }
    return;
}

void DrvEnableMotorControl(void)
{
	DRV_MOTOR_CONTEXT *mc;
	U16 data;
	U16 j;
	DrvFrozen=0;
	HmiQueryPulseAccumulator(&data);
	
	
	mc = &DrvpMotorContext;
	mc->expect = 0;
	mc->current = 0;
	for (j = 0; j < HISTORY_MAX; ++j) {
		mc->history[j] = data;
	}
	DrvpInitializePidController(&mc->param);
	
	(void)DRV_MEN_SetVal();
	(void)DRV_PWM_Enable();

	DrvpEnableControl = 1;
	return;
}

void DrvFreezeMotorControl(void){
    DRV_MOTOR_CONTEXT *mc;  
    DrvFrozen=1;  
    mc = &DrvpMotorContext; 
    DrvSavedExpect=mc->expect;
    DrvSavedCurrent=mc->current;
    mc->expect=0;
    mc->current=0;    
    return;
}


void DrvRestoreMotorControl(void){
    DRV_MOTOR_CONTEXT *mc;
    DrvFrozen=0;    
    mc = &DrvpMotorContext; 
    mc->expect=DrvSavedExpect;
    mc->current=DrvSavedCurrent; 
    return;   
}

void DrvDisableMotorControl(void){
    DrvpEnableControl = 0;
    DrvFrozen=1;
	(void)DRV_MEN_ClrVal();

	(void)DRV_PWM_Disable();   
	return;
}
void DrvSetSpeedMotor(S16 speed){
    DrvpMotorContext.expect=speed;  
    return;  
}

void DrvpSetPwmMotor(S16 pwm)
{
	U8 direction;
	U16 pwm0;
	
	if (pwm > 0) {
		direction = 1;
		pwm0 = ((U16)pwm)* 2 + 1;
	} else if (pwm == 0) {
		direction = 0;
		pwm0 = 65535;
	} else {
		direction = 0;
		pwm0 = ((U16)(pwm + 32767)) * 2;
	}
	
	DRV_BACK_PutVal(direction);
	(void)DRV_PWM_SetRatio16(pwm0);
	return;
}

static void DrvpMotorControl(U16 current)
{
	DRV_MOTOR_CONTEXT *mc = &DrvpMotorContext;
	S16 speed;
	S16 pwm;

//the line below will go wrong when delta of current and history > 32767
    speed = (S16)(current - mc->history[mc->current]);
   
	mc->history[mc->current] = current;
	if (++mc->current >= HISTORY_MAX) {
		mc->current = 0;
	}
	
	pwm = PID_controllerPIDtype1(mc->expect, speed, &mc->param);

	DrvpSetPwmMotor(pwm);
	return;
}

void DrvIrqHandler(void)
{
	U16 current;
	if(DrvFrozen){
	    DRV_MOTOR_CONTEXT *mc = &DrvpMotorContext;    
	    mc->expect=0;        
	}
	if (DrvpEnableControl) {
		HmiQueryPulseAccumulator(&current);
		DrvpMotorControl(current);
	}
	return;
}

void DRV_SERVO_SetDuty(U16 duty){
//    (void)DRV_SERVO_SetDutyUS(duty/*-IDV_SERVO_CORRECTION_MIDDLE+IdvServoCorrection*/); 
	DrvServoTarget = duty;
    return;
}

void DrvServoIrqHndler(void){
	static U16 current = DRV_SERVO_MIDDLE_DEFAULT;
	current = (current * DRV_SERVO_WEIGHT_OLD + DrvServoTarget * DRV_SERVO_WEIGHT_NEW) / DRV_SERVO_WEIGHT_ALL;
	(void)DRV_SERVO_SetDutyUS(current); 
	
}
/* END Drv */
